#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

class MavrosPositionSubscriber : public rclcpp::Node
{
public:
    MavrosPositionSubscriber() : Node("mavros_position_subscriber")
    {
        RCLCPP_INFO(this->get_logger(), "Mavros Position Subscriber");
        auto qos = rclcpp::QoS(rclcpp::SensorDataQoS());
        position_subscriber_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
        "/mavros/local_position/pose", qos, std::bind(&MavrosPositionSubscriber::positionCallback, this, std::placeholders::_1));
    }
    
private:
    void positionCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
    double x = msg->pose.position.x;
    double y = msg->pose.position.y;
    double z = msg->pose.position.z;

    RCLCPP_INFO(this->get_logger(), "Drone position: [x: %.2f, y: %.2f, z: %.2f]", x, y, z);
    }

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr position_subscriber_;
    };

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MavrosPositionSubscriber>());
    rclcpp::shutdown();
    return 0;
}